/*
 *  dc1394cam.c
 *  libCV
 *
 *  Created by Gregory Koch on 14/03/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#ifdef COMPIL_DC1394_CAM

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <dc1394/dc1394.h>
#include <stdlib.h>
#include <inttypes.h>
#include "dc1394cam.h"


/*-----------------------------------------------------------------------
 *  Releases the cameras and exits
 *-----------------------------------------------------------------------*/
static void cleanup_and_exit(dc1394camera_t *camera)
{
    dc1394_video_set_transmission(camera, DC1394_OFF);
    dc1394_capture_stop(camera);
    dc1394_camera_free(camera);
    exit(1);
}

static int cam_init(dc1394cam_t *cam)
{
    dc1394camera_list_t * list;
    dc1394error_t err;
    
    cam->d = dc1394_new ();
    if (!cam->d)
        return 1;
    err=dc1394_camera_enumerate (cam->d, &list);
    DC1394_ERR_RTN(err,"Failed to enumerate cameras");
    
    if (list->num == 0) {
        dc1394_log_error("No cameras found");
        return 1;
    }
    
    cam->camera = dc1394_camera_new (cam->d, list->ids[0].guid);
    if (!cam->camera) {
        dc1394_log_error("Failed to initialize camera with guid %llx", list->ids[0].guid);
        return 1;
    }
    dc1394_camera_free_list (list);
    
    printf("Using camera with GUID %"PRIx64"\n", cam->camera->guid);
    
    /*-----------------------------------------------------------------------
     *  setup capture
     *-----------------------------------------------------------------------*/
    
    err = dc1394_video_set_operation_mode(cam->camera,DC1394_OPERATION_MODE_LEGACY);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not set operation mode");
    
    err=dc1394_video_set_iso_speed(cam->camera, DC1394_ISO_SPEED_400);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not set iso speed");
    
    err=dc1394_video_set_mode(cam->camera, cam->format);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not set video mode\n");
    
    err=dc1394_video_set_framerate(cam->camera, DC1394_FRAMERATE_15);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not set framerate\n");
    
    err=dc1394_capture_setup(cam->camera, 4, DC1394_CAPTURE_FLAGS_AUTO_ISO);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not setup camera-\nmake sure that the video mode and framerate are not supported by your camera\n");
    
    /*-----------------------------------------------------------------------
     *  have the camera start sending us data
     *-----------------------------------------------------------------------*/
    err=dc1394_video_set_transmission(cam->camera, DC1394_ON);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not start camera iso transmission\n");
    
    return 0;
}

static int cam_grab_image(dc1394cam_t *cam)
{
    dc1394error_t err;
    dc1394video_frame_t *frame=NULL;
    int i;
    
    /*-----------------------------------------------------------------------
     *  capture one frame
     *-----------------------------------------------------------------------*/
    err=dc1394_capture_dequeue(cam->camera, DC1394_CAPTURE_POLICY_WAIT, &frame);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam->camera),"Could not capture a frame\n");
    
    if(cam->format == DC1394_VIDEO_MODE_160x120_YUV444 || 
       cam->format == DC1394_VIDEO_MODE_320x240_YUV422)
    {
        switch(cam->framebuf.type)
        {
            case IMG_RGB24:
                dc1394_convert_to_RGB8(frame->image, cam->framebuf.data, 
                                        cam->framebuf.size.width, cam->framebuf.size.height, 
                                        frame->yuv_byte_order,
                                        frame->color_coding, 
                                        frame->data_depth); 
                break;
                
            case IMG_GREYSCALE8:
                dc1394_convert_to_MONO8(frame->image, cam->framebuf.data, 
                                        cam->framebuf.size.width, cam->framebuf.size.height, 
                                        frame->yuv_byte_order,
                                        frame->color_coding, 
                                        frame->data_depth); 
                break;
        }
    }
    else
    {
        switch(cam->framebuf.type)
        {
            case IMG_RGB24:
                if(frame->stride == cam->framebuf.size.width*3)
                    memcpy(cam->framebuf.data, frame->image, 
                           3*cam->framebuf.size.width*cam->framebuf.size.height);
                else
                {
                    for(i=0; i<cam->framebuf.size.height; i++)
                        memcpy(cam->framebuf.data + 3*cam->framebuf.size.width*i, 
                               frame->image+frame->stride*i, 
                               3*cam->framebuf.size.width);
                }
                break;
                
            case IMG_GREYSCALE8:
                if(frame->stride == cam->framebuf.size.width)
                    memcpy(cam->framebuf.data, frame->image, 
                           cam->framebuf.size.width*cam->framebuf.size.height);
                else
                {
                    for(i=0; i<cam->framebuf.size.height; i++)
                        memcpy(cam->framebuf.data + cam->framebuf.size.width*i, 
                               frame->image+frame->stride*i, 
                               cam->framebuf.size.width);
                }
                break;
        }
    }
    
    dc1394_capture_enqueue (cam->camera, frame);
    
    return 0;
}

static int cam_free(dc1394cam_t cam)
{
    dc1394error_t err;
    
    /*-----------------------------------------------------------------------
     *  stop data transmission
     *-----------------------------------------------------------------------*/
    err=dc1394_video_set_transmission(cam.camera,DC1394_OFF);
    DC1394_ERR_CLN_RTN(err,cleanup_and_exit(cam.camera),"Could not stop the camera?\n");
    
    /*-----------------------------------------------------------------------
     *  close camera
     *-----------------------------------------------------------------------*/
    dc1394_video_set_transmission(cam.camera, DC1394_OFF);
    dc1394_capture_stop(cam.camera);
    dc1394_camera_free(cam.camera);
    dc1394_free (cam.d);
    
    return 0;
}


dc1394cam_t dc1394cam_open(unsigned int format)
{
    dc1394cam_t cam;
    
    switch(format)
    {
        case DC1394_RGB24_160_120:
            cam.framebuf = init_image(IMG_RGB24, init_size(160, 120));
            cam.format = DC1394_VIDEO_MODE_160x120_YUV444;
            break;
            
        case DC1394_GREY8_160_120:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(160, 120));
            cam.format = DC1394_VIDEO_MODE_160x120_YUV444;
            break;
            
        case DC1394_RGB24_320_240:
            cam.framebuf = init_image(IMG_RGB24, init_size(320, 240));
            cam.format = DC1394_VIDEO_MODE_320x240_YUV422;
            break;
            
        case DC1394_GREY8_320_240:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(320, 240));
            cam.format = DC1394_VIDEO_MODE_320x240_YUV422;
            break;
            
        case DC1394_RGB24_640_480:
            cam.framebuf = init_image(IMG_RGB24, init_size(640, 480));
            cam.format = DC1394_VIDEO_MODE_640x480_RGB8;
            break;
            
        case DC1394_GREY8_640_480:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(640, 480));
            cam.format = DC1394_VIDEO_MODE_640x480_MONO8;
            break;
            
        case DC1394_RGB24_800_600:
            cam.framebuf = init_image(IMG_RGB24, init_size(800, 600));
            cam.format = DC1394_VIDEO_MODE_800x600_RGB8;
            break;
            
        case DC1394_GREY8_800_600:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(800, 600));
            cam.format = DC1394_VIDEO_MODE_800x600_MONO8;
            break;
            
        default:
            fprintf(stderr, "libCV : dc1394cam : unknow format\n");
            exit(EXIT_FAILURE);
            break;
    }
    
    if(cam_init(&cam))
        exit(1);
    
    cam.fps = 0;    
    gettimeofday(&cam.tv, NULL);
    
    return cam;
}

void dc1394cam_close(dc1394cam_t camera)
{
    free_image(camera.framebuf);
    
    if(cam_free(camera))
        exit(1);
}

void dc1394cam_grab_image(dc1394cam_t *camera)
{
    struct timeval t;
    
    if(cam_grab_image(camera))
        exit(1);
    
    gettimeofday(&t, NULL);
    camera->fps = 1000000.0 / ( (t.tv_sec - camera->tv.tv_sec)*1000000.0 + (t.tv_usec - camera->tv.tv_usec));
    camera->tv = t;
}

#endif
